using BepuUtilities;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints.Contact
{
    /// <summary>
    /// 处理一个实体接触约束的切向摩擦实现。
    /// </summary>
    public static class TangentFrictionOneBody
    {
        public struct Jacobians
        {
            public Matrix2x3Wide LinearA;
            public Matrix2x3Wide AngularA;
        }
        public struct Projection
        {
            // 在飞翔上由切线和偏移生成雅可比。
            // 切线是从曲面基础重建的。
            // 这样,每个约束相对于两个共享的线性Jacobian和四个角Jacobian的半自然基线节省了11个浮点数。
            public Vector3Wide OffsetA;
            public Symmetric2x2Wide EffectiveMass;
        }

        // 因为这是一个非共享的专用实现,所以雅可比计算保存在这里,而不是批处理中。
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, out Jacobians jacobians)
        {
            // TODO：手动删除此副本有一个小小的好处,因为编译器很可能不会这样做,而且它可能还会引入更多的本地init。
            jacobians.LinearA.X = tangentX;
            jacobians.LinearA.Y = tangentY;
            Vector3Wide.CrossWithoutOverlap(offsetA, tangentX, out jacobians.AngularA.X);
            Vector3Wide.CrossWithoutOverlap(offsetA, tangentY, out jacobians.AngularA.Y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref BodyInertias inertiaA,
            out Projection projection)
        {
            ComputeJacobians(ref tangentX, ref tangentY, ref offsetA, out var jacobians);
            // 计算有效质量矩阵贡献。
            Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaA.InverseMass, out var linearContributionA);
            Symmetric3x3Wide.MatrixSandwich(jacobians.AngularA, inertiaA.InverseInertiaTensor, out var angularContributionA);

            // 没有软化;此约束在设计上是刚性的。(它确实支持最大力,但这与适当的阻尼比/固有频率不同。)
            Symmetric2x2Wide.Add(linearContributionA, angularContributionA, out var inverseEffectiveMass);
            Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out projection.EffectiveMass);
            projection.OffsetA = offsetA;

            // 请注意,摩擦约束没有偏移速度。他们的目标是零速度。
        }

        /// <summary>
        /// 将冲量从约束空间变换到世界空间,并使用它修改身体的缓存世界空间速度。
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA,
            ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA)
        {
            Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA);
            Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA);
            BodyVelocities correctiveVelocityA;
            Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear);
            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular);
            Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear);
            Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Projection projection, ref BodyInertias inertiaA,
            ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA)
        {
            ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, out var jacobians);
            // TODO：如果上一帧和当前帧与不同的时间步长相关联,则上一帧的解决方案将不再是一个好的解决方案。
            // 为了补偿这一点,如果DT改变,应该对累积的脉冲进行定标。
            ApplyImpulse(ref jacobians, ref inertiaA, ref accumulatedImpulse, ref wsvA);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref Projection data, ref Jacobians jacobians,
            ref Vector<float> maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI)
        {
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, jacobians.LinearA, out var csvaLinear);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Angular, jacobians.AngularA, out var csvaAngular);
            Vector2Wide.Add(csvaLinear, csvaAngular, out var csv);
            // 所需的校正速度是当前约束空间速度的负值。
            Symmetric2x2Wide.TransformWithoutOverlap(csv, data.EffectiveMass, out var negativeCSI);

            var previousAccumulated = accumulatedImpulse;
            Vector2Wide.Subtract(accumulatedImpulse, negativeCSI, out accumulatedImpulse);
            // 最大摩擦力取决于法向冲量。每次迭代都会提供最大值。
            Vector2Wide.Length(accumulatedImpulse, out var accumulatedMagnitude);
            // 注意零位保护的除法。
            var scale = Vector.Min(Vector<float>.One, maximumImpulse / Vector.Max(new Vector<float>(1e-16f), accumulatedMagnitude));
            Vector2Wide.Scale(accumulatedImpulse, scale, out accumulatedImpulse);

            Vector2Wide.Subtract(accumulatedImpulse, previousAccumulated, out correctiveCSI);

        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY,
            ref Projection projection, ref BodyInertias inertiaA, ref Vector<float> maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA)
        {
            ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, out var jacobians);
            ComputeCorrectiveImpulse(ref wsvA, ref projection, ref jacobians, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI);
            ApplyImpulse(ref jacobians, ref inertiaA, ref correctiveCSI, ref wsvA);

        }

    }
}
